#include "WxObjectOrientationMarkerInteractor.h"
#include <vtkPlane.h>
#include <vtkCamera.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkInteractorObserver.h>
#include <vtkTransform.h>
#include <itkEuler3DTransform.h>
#include <mitkLine.h>
#include <mitkNodePredicateDataType.h>
#include <mitkEvent.h>
#include <mitkDisplayPositionEvent.h>
#include <mitkDisplayGeometry.h>
#include <mitkGeometry2DData.h>
#include <mitkRenderingManager.h>
#include <mitkUndoController.h>
#include <mitkStateEvent.h>
#include <mitkState.h>
#include <mitkBaseRenderer.h>
#include <mitkInteractionConst.h>
#include <mitkAction.h>
#include <mitkProperties.h>


using namespace mitk;
WxObjectOrientationMarkerInteractor::WxObjectOrientationMarkerInteractor(const char * type,DataNode* objectOrientationMarkerNode):StateMachine(type),m_SelectedAxis(AXIS_NONE)
{
    m_ObjectOrientationMarkerNode=objectOrientationMarkerNode;
    if(m_ObjectOrientationMarkerNode.IsNotNull())
    {
        m_ObjectOrientationMarker=dynamic_cast<WxObjectOrientationMarker*>(m_ObjectOrientationMarkerNode->GetData()) ;
    }
    m_ViewPlaneGeometry=PlaneGeometry::New();
}

WxObjectOrientationMarkerInteractor::~WxObjectOrientationMarkerInteractor(void)
{

}

bool WxObjectOrientationMarkerInteractor::ExecuteAction( mitk::Action* action, mitk::StateEvent const* stateEvent )
{
    bool ok=false;
    if(m_ObjectOrientationMarker==NULL) return false;
    const mitk::Event *theEvent = stateEvent->GetEvent();
    mitk::ScalarType timeInMS = 0.0;
    if ( theEvent )
    {
        if (theEvent->GetSender() != NULL)
        {
            timeInMS = theEvent->GetSender()->GetTime();
        }
    }
    const DisplayPositionEvent* posEvent = dynamic_cast<const DisplayPositionEvent*>(stateEvent->GetEvent());
    if (!posEvent) return false;
    if (posEvent->GetSender()->GetMapperID()!=mitk::BaseRenderer::Standard3D) return false;
    bool f=false;
    m_ObjectOrientationMarkerNode->GetVisibility(f,theEvent->GetSender());
    if(!f) return false;

    switch (action->GetActionId())
    {

    case AcCHECKOBJECT:
        {
            if(m_ObjectOrientationMarker==NULL) return false;
            const Geometry3D* geometry=m_ObjectOrientationMarker->GetGeometry();
            if(geometry==NULL) return false;
            mitk::Point3D Ori=geometry->GetOrigin();
            if(this->IsMouseOnOrigin(Ori,posEvent->GetSender()->GetVtkRenderer(),posEvent->GetDisplayPosition()))
            {
                StateEvent *newStateEvent(NULL);
                newStateEvent = new StateEvent(EIDBODY, stateEvent->GetEvent());
                this->HandleEvent( newStateEvent );
                delete newStateEvent;
                return ok;
            }
            SelectedAxis sa=this->IsMouseOnAxis(posEvent->GetSender()->GetVtkRenderer(),posEvent->GetDisplayPosition());
            if(sa!=AXIS_NONE)
            {
                m_SelectedAxis=sa;
                StateEvent *newStateEvent(NULL);
                newStateEvent = new StateEvent(EIDTIP, stateEvent->GetEvent());
                this->HandleEvent( newStateEvent );
                delete newStateEvent;
                return ok;
            }
            ok = true;
            break;
        }
    case AcSELECTALL:
        {
            if(m_ObjectOrientationMarkerNode.IsNull()) return false;
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateXDiaplayColor", mitk::ColorProperty::New(1.0f,1.0f,0.5f) );
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateYDiaplayColor", mitk::ColorProperty::New(1.0f,1.0f,0.5f) );
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateZDiaplayColor", mitk::ColorProperty::New(1.0f,1.0f,0.5f) );
            mitk::BaseRenderer *renderer = posEvent->GetSender();
            vtkRenderWindow *renderWindow = NULL;
            vtkRenderWindowInteractor *renderWindowInteractor = NULL;
            if ( renderer != NULL )
            {
              renderWindow = renderer->GetRenderWindow();
              if ( renderWindow != NULL )
              {
                renderWindowInteractor = renderWindow->GetInteractor();
              }
            }
            renderWindowInteractor->Disable();

            RenderingManager::GetInstance()->ForceImmediateUpdateAll();

            ok = true;
            break;
        }
    case AcDESELECTALL:
        {
            if(m_ObjectOrientationMarkerNode.IsNull()) return false;
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateXDiaplayColor", mitk::ColorProperty::New(1.0f,0.0f,0.0f) );
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateYDiaplayColor", mitk::ColorProperty::New(0.0f,1.0f,0.0f) );
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateZDiaplayColor", mitk::ColorProperty::New(0.0f,0.0f,1.0f) );
            mitk::BaseRenderer *renderer = posEvent->GetSender();
            vtkRenderWindow *renderWindow = NULL;
            vtkRenderWindowInteractor *renderWindowInteractor = NULL;
            if ( renderer != NULL )
            {
              renderWindow = renderer->GetRenderWindow();
              if ( renderWindow != NULL )
              {
                renderWindowInteractor = renderWindow->GetInteractor();
              }
            }
            renderWindowInteractor->Enable();
            RenderingManager::GetInstance()->ForceImmediateUpdateAll();
            ok = true;
            break;
        }
    case AcCHECKPOINT:
        {
            if(this->IsMouseOnOrigin(m_OriginOld,posEvent->GetSender()->GetVtkRenderer(),posEvent->GetDisplayPosition())==false)
            {
                StateEvent *newStateEvent(NULL);
                newStateEvent = new StateEvent(EIDNO, stateEvent->GetEvent());
                this->HandleEvent( newStateEvent );
                delete newStateEvent;
                return ok;
            }
            ok = true;
            break;
        }
    case AcINITMOVE:
        {
            Vector3D v3dViewPlaneNormal;
            double *ViewPlaneNormal= posEvent->GetSender()->GetVtkRenderer()->GetActiveCamera()->GetViewPlaneNormal();
            vtk2itk(ViewPlaneNormal,v3dViewPlaneNormal);
            m_OriginOld=m_ObjectOrientationMarker->GetGeometry()->GetOrigin();
            m_ViewPlaneGeometry->InitializePlane(m_OriginOld,v3dViewPlaneNormal);
            Point3D cursor = posEvent->GetWorldPosition();
            Point2D cursor2d,origin2d;
            m_ViewPlaneGeometry->Map(cursor,cursor2d);
            m_ViewPlaneGeometry->Map(m_OriginOld,origin2d);
            m_CursorOriginOnViewPlane=cursor2d-origin2d;

            ok = true;
            break;
        }
    case AcMOVEPOINT:
        {
            Point3D cursor = posEvent->GetWorldPosition();
            Point2D cursorOnViewPlane,originOnViewPlane;
            m_ViewPlaneGeometry->Map(cursor,cursorOnViewPlane);
            originOnViewPlane=cursorOnViewPlane-m_CursorOriginOnViewPlane;

            Point3D originNew;
            m_ViewPlaneGeometry->Map(originOnViewPlane,originNew);

            this->MoveOrigin(originNew);
            ok = true;
            break;
        }
    case AcSELECTLINE:
        {
            if(m_ObjectOrientationMarkerNode.IsNull()) return false;
            if(m_SelectedAxis==AXIS_X)
                m_ObjectOrientationMarkerNode->SetProperty( "CoordinateXDiaplayColor", mitk::ColorProperty::New(1.0f,1.0f,0.5f) );
            if(m_SelectedAxis==AXIS_Y)
                m_ObjectOrientationMarkerNode->SetProperty( "CoordinateYDiaplayColor", mitk::ColorProperty::New(1.0f,1.0f,0.5f) );
            if(m_SelectedAxis==AXIS_Z)
                m_ObjectOrientationMarkerNode->SetProperty( "CoordinateZDiaplayColor", mitk::ColorProperty::New(1.0f,1.0f,0.5f) );
            mitk::BaseRenderer *renderer = posEvent->GetSender();
            vtkRenderWindow *renderWindow = NULL;
            vtkRenderWindowInteractor *renderWindowInteractor = NULL;
            if ( renderer != NULL )
            {
              renderWindow = renderer->GetRenderWindow();
              if ( renderWindow != NULL )
              {
                renderWindowInteractor = renderWindow->GetInteractor();
              }
            }
            renderWindowInteractor->Disable();

            RenderingManager::GetInstance()->ForceImmediateUpdateAll();

            ok = true;
            break;
        }
    case AcDESELECTLINE:
        {
            if(m_ObjectOrientationMarkerNode.IsNull()) return false;
            m_SelectedAxis=AXIS_NONE;
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateXDiaplayColor", mitk::ColorProperty::New(1.0f,0.0f,0.0f) );
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateYDiaplayColor", mitk::ColorProperty::New(0.0f,1.0f,0.0f) );
            m_ObjectOrientationMarkerNode->SetProperty( "CoordinateZDiaplayColor", mitk::ColorProperty::New(0.0f,0.0f,1.0f) );
            mitk::BaseRenderer *renderer = posEvent->GetSender();
            vtkRenderWindow *renderWindow = NULL;
            vtkRenderWindowInteractor *renderWindowInteractor = NULL;
            if ( renderer != NULL )
            {
              renderWindow = renderer->GetRenderWindow();
              if ( renderWindow != NULL )
              {
                renderWindowInteractor = renderWindow->GetInteractor();
              }
            }
            renderWindowInteractor->Enable();
            RenderingManager::GetInstance()->ForceImmediateUpdateAll();
            ok = true;
            break;
        }
    case AcCHECKLINE:
        {
          if(this->IsMouseOnAxis(posEvent->GetSender()->GetVtkRenderer(),posEvent->GetDisplayPosition())!=this->m_SelectedAxis)
            {
                StateEvent *newStateEvent(NULL);
                newStateEvent = new StateEvent(EIDNO, stateEvent->GetEvent());
                this->HandleEvent( newStateEvent );
                delete newStateEvent;
                return ok;
            }
            ok = true;
            break;
        }
    case AcINITMOVEMENT:
        {
            m_OriginOld=m_ObjectOrientationMarker->GetGeometry()->GetOrigin();
            m_CursorOld2D=posEvent->GetDisplayPosition();
            ok = true;
            break;
        }
    case AcROTATE:
        {
            mitk::WxObjectOrientationMarker* data=m_ObjectOrientationMarker;
            if (data)
            {
              Point2D cursor2d = posEvent->GetDisplayPosition();
              float angle=cursor2d[1]-m_CursorOld2D[1];
              mitk::Vector3D axis;
              if(m_SelectedAxis==AXIS_X)
              {
                 axis=m_ObjectOrientationMarker->GetGeometry()->GetAxisVector(0);
                 m_CursorOld2D=cursor2d;
                 RotateWXYZ( axis,angle);
              }
              if(m_SelectedAxis==AXIS_Y)
              {
                  axis=m_ObjectOrientationMarker->GetGeometry()->GetAxisVector(1);
                  m_CursorOld2D=cursor2d;
                  RotateWXYZ( axis,angle);
              }
              if(m_SelectedAxis==AXIS_Z)
              {
                  axis=m_ObjectOrientationMarker->GetGeometry()->GetAxisVector(2);
                  m_CursorOld2D=cursor2d;
                  RotateWXYZ( axis,angle);
              }

          }
            ok = true;
            break;
        }
    case AcMOVESELECTED:
        {
            float lineLength=50;
            if (dynamic_cast<mitk::FloatProperty *>(m_ObjectOrientationMarkerNode->GetProperty("CoordinateDiaplayLength")) != NULL)
                lineLength = dynamic_cast<mitk::FloatProperty*>(m_ObjectOrientationMarkerNode->GetProperty("CoordinateDiaplayLength"))->GetValue();
            lineLength*=0.025;
            mitk::WxObjectOrientationMarker* data=m_ObjectOrientationMarker;
            if (data)
            {
              Point2D cursor2d = posEvent->GetDisplayPosition();
              float distance=cursor2d[1]-m_CursorOld2D[1];
              mitk::Vector3D axis;
              if(m_SelectedAxis==AXIS_X)
              {
                 axis=m_ObjectOrientationMarker->GetGeometry()->GetAxisVector(0);
                 axis.Normalize();
                 Point3D p3d=m_OriginOld+axis*distance*lineLength;
                 MoveOrigin(p3d);
              }
              if(m_SelectedAxis==AXIS_Y)
              {
                  axis=m_ObjectOrientationMarker->GetGeometry()->GetAxisVector(1);
                  axis.Normalize();
                  Point3D p3d=m_OriginOld+axis*distance*lineLength;
                  MoveOrigin(p3d);
              }
              if(m_SelectedAxis==AXIS_Z)
              {
                  axis=m_ObjectOrientationMarker->GetGeometry()->GetAxisVector(2);
                  axis.Normalize();
                  Point3D p3d=m_OriginOld+axis*distance*lineLength;
                  MoveOrigin(p3d);
              }

          }
            RenderingManager::GetInstance()->ForceImmediateUpdateAll();
            ok = true;
            break;

        }
    default:  return Superclass::ExecuteAction( action, stateEvent );
    }

    return ok;
}


void WxObjectOrientationMarkerInteractor::RotateWXYZ(mitk::Vector3D axisOfRotation,double angle)
{
    mitk::WxObjectOrientationMarker* data=m_ObjectOrientationMarker;
    if (data)
    {
        mitk::Geometry3D* g3d=data->GetGeometry();
//        mitk::Vector3D vx=g3d->GetAxisVector(0); MITK_INFO<<"vx"<<vx;
//        mitk::Vector3D vy=g3d->GetAxisVector(1); MITK_INFO<<"vy"<<vy;
//        mitk::Vector3D vz=g3d->GetAxisVector(2); MITK_INFO<<"vz"<<vz;

        mitk::Point3D center= g3d->GetOrigin();
        vtkMatrix4x4* translationMatrix = vtkMatrix4x4::New();
        translationMatrix->Identity();
        double (*transMatrix)[4] = translationMatrix->Element;
        transMatrix[0][3] = center[0];
        transMatrix[1][3] = center[1];
        transMatrix[2][3] = center[2];
        translationMatrix->Invert();
        g3d->Compose( translationMatrix );

//        mitk::Vector3D vx1=g3d->GetAxisVector(0); MITK_INFO<<"vx1"<<vx1;
//        mitk::Vector3D vy1=g3d->GetAxisVector(1); MITK_INFO<<"vy1"<<vy1;
//        mitk::Vector3D vz1=g3d->GetAxisVector(2); MITK_INFO<<"vz1"<<vz1;


        vtkTransform *vtktransform = vtkTransform::New();
        vtktransform->Identity();
        vtktransform->RotateWXYZ(angle, axisOfRotation[0], axisOfRotation[1], axisOfRotation[2]);
        vtkMatrix4x4* rotationMatrix=vtktransform->GetMatrix();
        g3d->Compose(rotationMatrix);

//        mitk::Vector3D vx2=g3d->GetAxisVector(0); MITK_INFO<<"vx2"<<vx2;
//        mitk::Vector3D vy2=g3d->GetAxisVector(1); MITK_INFO<<"vy2"<<vy2;
//        mitk::Vector3D vz2=g3d->GetAxisVector(2); MITK_INFO<<"vz2"<<vz2;

        translationMatrix->Invert();
        g3d->Compose( translationMatrix );
        data->Modified();
        RenderingManager::GetInstance()->ForceImmediateUpdateAll();
        vtktransform->Delete();
        translationMatrix->Delete();


    }
}

bool WxObjectOrientationMarkerInteractor::IsMouseOnOrigin(const mitk::Point3D origin, vtkRenderer* renderer,const mitk::Point2D cursor2d,float error)
{

    double vtkOrigin2d[3];
    mitk::Point2D ori2d;
    if(renderer==NULL) return false;
    renderer->SetWorldPoint(origin[0],origin[1],origin[2],1.0);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(vtkOrigin2d);
    ori2d[0]=vtkOrigin2d[0];
    ori2d[1]=vtkOrigin2d[1];
    mitk::Vector2D v2d=ori2d-cursor2d;
//    MITK_INFO<<vtkOrigin2d[0]<<"  "<<vtkOrigin2d[1];
//    MITK_INFO<<cursor2d[0]<<"  "<<cursor2d[1];
//    MITK_INFO<<v2d.GetNorm();
    if(v2d.GetNorm()<error)
        return true;
    else
        return false;
}


void WxObjectOrientationMarkerInteractor::MoveOrigin(mitk::Point3D p3d)
{
    mitk::WxObjectOrientationMarker* data=m_ObjectOrientationMarker;
    if (data)
    {
        data->GetGeometry()->SetOrigin(p3d);
        data->GetGeometry()->Modified();
        data->Modified();
    }
}
WxObjectOrientationMarkerInteractor::SelectedAxis
WxObjectOrientationMarkerInteractor::IsMouseOnAxis( vtkRenderer* renderer,const mitk::Point2D cursor2d,float error)
{
    if(m_ObjectOrientationMarker==NULL||m_ObjectOrientationMarkerNode.IsNull()) return AXIS_NONE;
    float lineLength=50;
    if (dynamic_cast<mitk::FloatProperty *>(m_ObjectOrientationMarkerNode->GetProperty("CoordinateDiaplayLength")) != NULL)
        lineLength = dynamic_cast<mitk::FloatProperty*>(m_ObjectOrientationMarkerNode->GetProperty("CoordinateDiaplayLength"))->GetValue();
    mitk::Geometry3D *g3d=m_ObjectOrientationMarker->GetGeometry();
    mitk::Vector3D AxisX=g3d->GetAxisVector(0); AxisX.Normalize();
    mitk::Vector3D AxisY=g3d->GetAxisVector(1); AxisY.Normalize();
    mitk::Vector3D AxisZ=g3d->GetAxisVector(2); AxisZ.Normalize();
    mitk::Point3D origin=g3d->GetOrigin();
    mitk::Point3D axisX3d=origin+AxisX*lineLength;
    mitk::Point3D axisY3d=origin+AxisY*lineLength;
    mitk::Point3D axisZ3d=origin+AxisZ*lineLength;

    mitk::Point2D ori2d,axisX2d,axisY2d,axisZ2d;
    if(renderer==NULL) return AXIS_NONE;
    double vtkTemp[3];
    renderer->SetWorldPoint(origin[0],origin[1],origin[2],1.0);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(vtkTemp);
    ori2d[0]=vtkTemp[0];ori2d[1]=vtkTemp[1];
    renderer->SetWorldPoint(axisX3d[0],axisX3d[1],axisX3d[2],1.0);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(vtkTemp);
    axisX2d[0]=vtkTemp[0];axisX2d[1]=vtkTemp[1];
    if(IsOn2DLine(ori2d,axisX2d,cursor2d,error))
        return AXIS_X;

     renderer->SetWorldPoint(axisY3d[0],axisY3d[1],axisY3d[2],1.0);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(vtkTemp);
    axisY2d[0]=vtkTemp[0];axisY2d[1]=vtkTemp[1];
    if(IsOn2DLine(ori2d,axisY2d,cursor2d,error))
        return AXIS_Y;

    renderer->SetWorldPoint(axisZ3d[0],axisZ3d[1],axisZ3d[2],1.0);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(vtkTemp);
    axisZ2d[0]=vtkTemp[0];axisZ2d[1]=vtkTemp[1];
    if(IsOn2DLine(ori2d,axisZ2d,cursor2d,error))
        return AXIS_Z;

    return AXIS_NONE;
}
bool WxObjectOrientationMarkerInteractor::IsOn2DLine(mitk::Point2D begin,mitk::Point2D end,mitk::Point2D cursor2d,float error)
{
    Line<ScalarType,2> *needle = new Line<ScalarType,2>();
    needle->SetPoints(begin, end);
    float length=(begin-end).GetNorm();
    float thisDistance = needle->Distance(cursor2d);
    float disToEnd=(end-needle->Project(cursor2d)).GetNorm();
    float disToOri=(begin-needle->Project(cursor2d)).GetNorm();
//        MITK_INFO<<begin[0]<<"  "<<begin[1];
//        MITK_INFO<<end[0]<<"  "<<end[1];
//        MITK_INFO<<cursor2d[0]<<"  "<<cursor2d[1];
//        MITK_INFO<<thisDistance;
    if (thisDistance<error&&disToEnd<=length&&disToOri<=length)
        return true;
    else
        return false;
    delete needle;
}
